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Abstract  -  When  a  mobile  robot  is  executing  a  navigational 
task  in  an  urban  outdoor  environment,  accurate  localization 
information  is  often  essential.  The  difficulty  of  this  task  is 
compounded  by  sensor  drop-out  and  the  presence  of  non-linear 
error  sources  over  the  span  of  the  mission.  We  have  observed  that 
certain  motions  of  the  robot  and  environmental  conditions  affect 
pose  sensors  in  different  ways.  In  this  paper,  we  propose  a 
computational  method  for  localization  that  systematically 
integrates  and  evaluates  contextual  information  that  affects  the 
quality  of  sensors,  and  utilize  the  information  in  order  to  Improve 
the  output  of  sensor  fusion.  Our  method  was  evaluated  in 
comparison  with  conventional  probabilistic  localization  methods 
(namely,  the  extended  Kalman  filter  and  Monte  Carlo 
localization)  in  a  set  of  outdoor  experiments.  The  results  of  the 
experiment  are  also  reported  in  this  paper. 

Index  Terms  -  Sensor  Fusion,  Context-Sensitive  Perception, 
Localization,  Extended  Kalman  Filter,  Particle  Filter 

I.  Introduction 

Execution  of  an  autonomous  mobile  robot  mission  in  an 
urban  outdoor  environment,  such  as  a  reconnaissance  mission, 
often  requires  a  waypoint-following  capability  (e.g.  [1,  2]). 
Since  waypoints  are  generally  specified  in  a  world  coordinate 
system,  localizing  the  robot  relative  to  the  world  coordinate 
system  is  vital  in  such  applications.  Furthermore,  depending  on 
the  urgency  of  the  mission,  the  localization  may  have  to  be 
accomplished  as  rapidly  as  possible.  Today,  in  a  typical  well- 
structured  indoor  environment,  laser-based  and  vision-based 
SLAM  approaches  have  proven  to  be  useful  for  localization 
[3-6].  However,  objects  in  an  outdoor  environment  are  less 
structured,  and  the  lasers  or  vision  systems  are  vulnerable  to 
severe  and  dynamic  variations  in  lighting.  For  outdoor 
applications,  the  global  positioning  system  (GPS),  compass, 
gyro-based  inertial  measurement  unit  (IMU),  and  shaft- 
encoder  are  standard  sensors  used  to  measure  where  the  robot 
is  and  what  direction  it  is  heading.  On  the  other  hand,  these 
sensors  are  still  not  perfect,  each  having  its  own  peculiar 
strengths  and  weaknesses.  For  example  in  the  case  of  a 
differential  GPS,  if  satellite  signals  and/or  differential  signals 
are  disrupted  by  the  surrounding  environment,  the  positional 
accuracy  provided  by  the  GPS  severely  degrades.  However, 
our  supposition  here  is  that  the  robot  should  be  able  to 


dynamically  assess  the  qualities  of  the  sensors  by  monitoring 
parameters  that  are  known  to  affect  them  (e.g.,  RT-20  value 
[7]  of  the  GPS,  speed  of  the  robot,  etc.).  We  refer  to  such 
parameters  as  contextual  information.  The  contextual 
information  is  translated  into  some  quantities  that  convey  the 
qualities  of  the  sensors  by  applying  some  domain  knowledge 
(rules). 

The  objective  of  this  paper  is  twofold:  (1)  To  propose  a 
computational  scheme  that  can  systematically  incorporate 
contextual  information  and  domain  knowledge  in  order  to 
gauge  the  robot’s  best  current  pose  when  measurements  from 
multiple  sensors  are  available;  and  (2)  to  empirically  evaluate 
whether  incorporation  of  such  information/knowledge  is 
indeed  useful  or  not. 

It  should  be  noted  that,  in  this  paper,  the  term  “pose”  we 
are  referring  to  is  a  six-dimensional  vector  (X),  whose 
components  includes,  y,  z,  <j>,  9,  and  (iZ (Equation  1): 

X=\x  y  z  (j)  9  y/f  (i) 

The  values  of  x,  y,  and  z  describe  the  location  of  a  body  in  the 
world  coordinate  system,  and  their  units  are  in  meters.  The  x- 
axis  points  to  East,  and  the  y-axis  points  to  North.  The  value  z 
describes  an  altitude.  On  the  other  hand,  rotations  <p,  9,  and  y/ 
are  yaw,  pitch,  and  roll  of  the  body,  respectively,  in  degrees. 

The  process  of  collecting  data  from  multiple  sensors  in 
order  to  produce  an  integrated  perceptual  output  is  known  as 
sensor  fusion.  Murphy  [8],  for  example,  proposed  a  perceptual 
architecture  (SFX)  for  action-oriented  sensor  fusion.  In  the 
SFX,  one  of  three  fusion  states  is  selected  based  on  behavioral 
needs.  In  the  first  fusion  state,  contributions  from  all  sensors 
are  integrated  in  order  to  produce  a  single  perceptual  output 
since  no  conflict  among  different  sensors  is  expected.  In  the 
second  fusion  state,  a  conflict  (discordance)  among  the  sensors 
is  expected,  and  calibration  of  weak  (unreliable)  sensors  is 
performed  before  generating  the  overall  output.  The  third  type 
is  a  greedy  method.  The  discordance  among  the  sensors  is 
resolved  by  suppressing  the  contributions  from  all  sensors 
except  the  most  reliable  one.  In  our  three  proposed  fusing 
methods  (Section  II. B):  two  of  them  (the  EKF  and  Particle 
Filter)  relate  to  Murphy’s  first  fusion  state  (cooperative);  and 
one  of  them  (the  Maximum  Confidence)  relates  to  the  third 
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fusion  state  (suppressive).  Furthermore,  our  cooperative  fusing 
methods  incorporate  probahilities,  as  the  employment  of 
probabilities  in  the  context  of  sensor  fusion  for  mobile  robots 
has  proven  useful  for  more  than  a  decade  [9].  In  fact,  Thrun 
[10]  reports  in  his  survey  paper  that  all  of  the  successful 
approaches  to  robotics  localization  today  employ  probabilistic 
techniques. 

The  organization  of  this  paper  is  as  follows.  An  overview 
of  the  context-sensitive  pose  computation  is  represented  in 
Section  II.  The  implementation  details  of  the  computational 
scheme  are  described  in  Section  III.  Empirical  evaluation  via 
outdoor  experimentation  is  presented  in  Section  IV.  Finally, 
conclusions  and  future  work  are  discussed  in  Section  V. 

II.  Context-Sensitive  Pose  Computation 
A.  Computational  Steps 

Suppose  that  a  mobile  robot  is  equipped  with  multiple 
pose  sensors  (e.g.,  GPS,  compass,  IMU,  shaft-encoder,  etc.). 
Each  sensor  provides  pose  information  that  may  be  computed 
in  different  coordinate  systems  and  generally  updated  at  a 
different  time  rate  from  others.  Given  these  sensors,  the 
objective  here  is  to  compute  the  best  pose  at  any  given  time  in 
the  common  world  coordinate  system. 

The  first  step  in  the  process  is  to  transform  the  original 
sensor  readings  measured  by  each  sensor  into  the  common 
world  coordinate  system.  Since  different  sensors  operate  with 
different  coordinate  systems,  the  transformation  function  is 
unique  to  the  sensor  type.  It  should  be  noted  that  some  sensors, 
such  as  shaft-encoders  and  IMU,  compute  their  poses  based  on 
dead-reckoning.  While  such  sensors  are  prone  to  cumulative 
errors,  their  incremental  readings  from  previous  measurement 
are  reasonably  accurate.  The  other  types  of  sensors,  such  as 
compass  and  GPS,  compute  poses  with  the  absolute  location 
and  orientation.  Equation  2  expresses  this  operation 
mathematically: 

{/ti  )  if  ^  =  dead-reckoning-based 

(2) 

/t  *('■*()  otherwise 

where  is  the  converted  pose  for  sensor  ^,  t  is  the  current 
instant,  /xj  is  the  transformation  function  for  the  sensor,  Ar,  is 
the  increment  from  the  previous  measurement,  and  is  the 
measurement  itself. 

Once  a  pose  from  each  sensor  is  computed,  the  next  step  is 
to  compute  estimated  accuracy  of  the  pose;  in  this  paper,  we 
refer  to  the  estimated  accuracy  as  grade,  denoted  with  letter  G. 
More  specifically,  suppose  we  define  vectors  <P  and  27  as 
shown  in  Equations  3  and  4,  respectively: 

0  =  [a^  (3) 

where  is  a  confidence  value  that  is  a  scalar  value  ranging 
from  0  to  1  to  quantify  how  certain  the  value  x  is,  and: 

_2  _2  _2  _2  _2  -iT  , 

where  (Tx  is  a  variance  of  x.  The  grade  is  then  defined  as  a 
matrix  (6x2)  that  contains  both  vectors  (Equation  5): 


G  =  [0  X]  (5) 

Both  confidence  values  and  variance  are  adjusted  depending 
on  status  of  the  sensor  and/or  robot.  For  example,  as 
mentioned  earlier,  when  the  differential  signals  are  disrupted, 
the  pose  computed  by  the  GPS  should  be  considered 
inaccurate;  hence,  the  confidence  values  should  be  degraded, 
and  the  variances  should  be  increased,  accordingly.  Indeed, 
assigning  these  values  requires  some  real-time  information  that 
measures  status  of  the  sensor  and/or  robot.  Furthermore,  it  also 
requires  some  rule-based  knowledge  that  translates  the 
sensor/robot  status  into  the  grade  of  the  pose.  We  refer  to  the 
real-time  information  of  the  sensor/robot  as  contextual 
information  (vector  C)  and  the  rule-based  knowledge  as 
domain  knowledge  (function  /d).  G  for  sensor  5  is  hence 
computed  by  Equation  6: 

G.=/d.(CJ  (6) 

Different  sensors  require  different  types  of  contextual 
information  and  domain  knowledge  to  compute  the  grades. 
Thus,  the  contents  of  C  and  the  rules  in  /d  are  unique  to  the 
sensor  type  (s).  Specific  values/rules  of  C  and  /d  employed 
during  the  experiment  conducted  for  this  paper  is  shown  in 
Section  IV. C. 

At  this  point,  we  have  (pose)  and  G^  (grade)  for  all 
available  sensors.  The  next  step  is  to  adjust  the  confidence 
values  (a)  in  Gs  based  on  the  types  of  sensors.  Our  assumption 
here  is  that  even  when  a  particular  sensor  is  operating  at  its 
best  condition  (hence  100%  confidence  value),  this  sensor  may 
be  known  to  be  unreliable  if  compared  with  other  sensors. 
Discounting  of  the  a  value  in  Gj  can  be  done  by  Equation  7: 

G:=G,r,  (7) 

where  Gj  is  the  discounted  grade,  and  Fj  is  an  weighting 
matrix  that  includes  predefined  discount  factor  %  ranging  from 
0  to  1  (Equation  8): 


(See  Section  IV. C.  for  specific  values  used  for  y  in  our 
experiment.) 

Given  X^  and  Gf  for  n  pose  sensors  that  have  updated  their 
measurements  since  the  previous  time  cycle,  the  last  step  here 
is  to  compute  the  best  estimate  of  the  current  pose  from  those. 
We  refer  to  this  step  as  fusing,  and  it  is  described  by: 

= fn  ,  g:„  , ... ,  g:,  )  (9) 

where  X^  is  a  fused  pose  (the  best  estimate  of  the  current  pose) 
for  the  current  instant  (t);  and/p^  is  a  function  that  computes 
the  fused  pose  using  fusing  method  Currently,  we  have 
implemented  three  types  of  fusing  methods,  which  are 
explained  in  the  following  section. 

It  should  be  noted  that  our  computational  method  here 
supports  asynchrony  of  sensor  measurements.  In  other  words, 
different  sensors  may  have  different  update  frequencies  (e.g., 
the  GPS  usually  updates  less  frequently  than  the  IMU  does). 
Thus,  n  in  Equation  9  can  be  different  from  one  cycle  to 
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another.  Furthermore,  it  should  be  also  noted  that  dynamically 
adjusting  the  contents  of  G-matrix  (Equation  6)  is  our 
definition  of  context-sensitive  pose  computation.  In  order  to 
determine  how  effective  it  is  to  incorporate  dynamically 
changing  contextual  information  upon  pose  estimation,  we 
evaluated  the  computational  scheme  where  the  G-matrix  was 
dynamically  computed  through  Equation  6  against  a  control 
scheme  where  the  G-matrix  was  statically  specified  (Section 
IV). 

B.  Fusing  Methods 

As  mentioned  above,  function  computes  the  output 
pose  (X^)  given  the  input  poses  from  different  sensors  and  their 
associated  grades  using  fusing  method  ^  (Equation  9).  In  this 
section,  we  describe  how  the  fusing  function  can  be 
constructed  in  this  framework  by  showing  how  the  three  types 
of  our  fusing  methods  (Maximum  Confidence,  Extended 
Kalman  Eilter,  and  Particle  Eilter)  are  implemented. 

1 .  Maximum  Confidence 

The  first  fusing  method,  called  Maximum  Confidence,  is 
essentially  a  greedy  method.  Each  element  in  the  fused  pose  is 
copied  from  the  one  whose  confidence  value  is  the  highest 
among  all  candidates.  Expressing  mathematically,  let  us  first 
define  vector  a'"  as  an  n-length  vector  that  contains  the 
element  of  confidence  values  stored  in  the  discounted  G- 
matrices  (Equation  7)  for  all  n  sensors: 

a"'=[Glfm,l)  ■■■  G;(m,l)f  (10) 

Eor  example,  a*  is  a  vector  that  contains  confidence  values  of  x 
for  all  n  sensors,  and  a*  is  the  same  for  (f).  The  process  of 
fusing  via  the  Maximum  Confidence  can  be  then  computed  by 
Equation  1 1 : 

,  ^(1)  ,  2,(2)  ,  ,fd)f  (11) 

i  IMax(a  )  ^  ^  IMax(a  )  ^  '  IMax(fl  )  ^  ^  ' 

where  IMax  is  a  function  that  takes  a  vector  and  returns  the 
index  of  the  element  that  has  the  largest  value;  d  is  the 
dimension  of  A  (<7  =  6  in  our  case).  In  the  context-sensitive 
pose  computation,  dynamic  adjustment  of  the  G-matrix 
(Equation  6)  affects  the  values  of  a"',  and  thus  it  influences  the 
selection  of  pose-components. 

2.  Extended  Kalman  Eilter 

The  second  type  of  fusing  methods  incorporates  the 
Extended  Kalman  Eilter  (EKE)  as  means  to  estimate  the 
current  pose  probabilistically.  The  EKE,  an  extension  of  the 
simple  Kalman  filter  used  to  handle  nonlinearities,  is  a 
recursive  filter  that  estimates  the  current  state  of  a  system. 
Because  sensor  fusion  can  be  suitably  handled  by  its 
mathematical  formulation  [9],  both  the  simple  Kalman  filter 
and  EKE  have  been  a  popular  choice  in  mobile  robot 
localization  [11-14].  The  pose  computation  via  the  EKE  has 
two  distinct  phases  {prediction  and  update),  which  are 
explained  below.  (See  [15]  for  more  details  including  the 
theoretical  background.) 


In  the  prediction  phase,  the  estimated  pose  (A^)  at  the 
current  instant  t  based  on  the  process  (motion)  model  is  first 
projected  (Equation  12): 

^p,=  (12) 


/pE  is  a  function  that  implements  the  process  model;  the 
function  takes  the  fused  pose  computed  in  the  previous  cycle 
(Af(.i),  control  input  (m,),  and  estimated  process  noise  (wm).  m, 
is  estimated  using  shaft-encoder  readings  {Xr^hatf)  since  the 
shaft-encoder  is  a  standard  sensor  that  is  installed  on  most  of 
the  mobile  robots  in  our  laboratory.  Erom  simple  kinematics, 
the  process  model  can  be  described  by  the  following  equation 
(Equation  13): 


^pt  -  ^ct-i  + 


A/cos(6>^,_i  -t^)cos((/>^,^j  -t^) 

xe,  x<t>, 

A/  cos(6»^,_i  -I-  —^)  &m{<t)^,_i  +  —^) 
A/9 


+  w,_i  (13) 


xe, 

Xy, 


where  XI  is  the  Euclidean  norm  of  Xr^haft  (Equation  14);  Xtj), 
X0,  and  A  ((rare  (j),  6,  and  ((r components  of  Xr shaft,  respectively. 
The  values  of  w  are  all  approximated  as  zero  in  this  phase: 

XI  =  yjXx^  -vXy^  -vXz^  (14) 

In  the  prediction  phase,  the  estimated  error  covariance 
matrix  (P)  is  also  projected  (Equation  15): 

P,  =  A  P,_^  -VW  QW'^  (15) 

where  A  is  the  Jacobian  of  /pe  with  respect  to  A;  W  is  the 
Jacobian  of  /pe  in  relation  to  w;  and  Q  is  the  process  noise 
covariance.  It  should  be  noted  that  the  P-matrix  is  recursively 
computed  using  values  from  the  previous  cycle.  More 
specifically,  suppose  we  split  up/pE  in  terms  of  the  elements  of 
the  pose  to  be  computed  (Equation  16): 

/pe  “  [/pEx  f?Ey  ■■■  /pE^] 

By  the  definition  of  the  Jacobian  matrix,  the  A-matrix  in 
Equation  15  can  be  then  expressed  as: 


^fpEx 

^fpEx 

dx 

dtp 

^/pE^ 

^/pEft 

dx 

dtp 

Because  of  Equation  13,  the  Vk-matrix  in  Equation  15  is  a  6x6 
identity  matrix  in  this  case.  The  Q-matrix  is  computed  by 
Equation  18: 

Q  =  Diag(A^^^, )  =  Diag(G;^^,  (:,  2))  (18) 

where  Diag  is  a  function  that  returns  a  square  matrix  whose 
diagonal  elements  are  copies  of  elements  in  the  input  vector 
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(other  elements  are  set  to  be  zero);  and  Eshaft  is  the  variances  of 
shaft-encoder  readings  stored  in  the  second  column  of  the 
discounted  G-matrix  (the  colon  indicates  that  all  elements  are 
taken  into  account). 

In  the  update  phase  of  the  EKF,  the  Kalman  gain  (K)  and 
final  estimates  of  the  current  pose  (X^)  and  the  error 
covariance  (P)  are  computed  based  on  the  latest  measurements 
(Xs  calculated  by  Equation  2  and  arrived  to  the  fuser  via 
Equation  9). 

Let  us  define  a  function  (/hj)  that  computes  an  estimated 
sensor  measurement  (Xh)  for  sensor  s  from  an  input  pose  (X) 
and  its  estimated  error  (v): 

X,.,=fnAX,v)  (20) 


fuse  sensor  readings  and  represent  likelihood  distributions 
over  localization  space  [5,  16-18].  As  in  the  EKF,  the  Particle 
Filter  computes  the  current  pose  (X^)  using  both  the  prediction 
and  update  phases.  (A  good  review  of  the  MCE  method 
including  the  theoretical  justification  can  be  found  in  [5,  19].) 
To  carry  out  the  computation,  the  Particle  Filter  utilizes  a 
concept  called  “particle”,  which  is  essentially  a  six¬ 
dimensional  pose  (o)  obtained  from  sampling  methods 
described  below.  As  a  fuser,  the  Particle  Filter  retains  a  large 
number  (N)  of  particles  to  compute  the  final  pose.  Here,  we 
use  O,  a  6'xN  matrix,  to  denote  the  set  of  N  particles  (Equation 
21): 

0  =  {0^  O2  0„]  (27) 


Assuming  the  coordinate  systems  of  A/,  and  X  are  the  same. 
Equation  20  can  be  replaced  with  a  simple  linear  equation 
(Equation  21): 

/h,(A,v)  =  A+v  (21) 


Given  /hj  and  P  (from  Equation  15),  the  Kalman  gain  for 
sensor  s  is  then  computed  by  Equation  22: 


K,  = 


PH, 


(22) 


where  H  is  the  Jacobian  of/n  with  respect  to  X  (i.e.,  an  identity 
matrix  in  our  case  because  of  Equation  21),  V  is  the  Jacobian 
of  /h  with  respect  to  v  (also  an  identity  matrix),  and  R  is  the 
sensor  noise  covariance  for  the  sensor  (Equation  23): 

=  Diag(i:j  =  Diag(Gj(:,  2))  (23) 


Finally,  given  the  Kalman  gains  for  all  n  sensors,  the  fused 
pose  (the  final  estimate  of  the  current  pose)  of  the  EKF  is 
computed  by  the  following  equation: 


seS  r\s^shaft 


(24) 


where  5  is  a  set  of  all  n  sensors  that  have  updated  their 
measurements  since  the  previous  cycle.  Substituting  with 

Equation  21,  Equation  24  can  be  simplified  as  Equation  25  in 
our  implementation: 

ZKAX.-X^-vJ  (25) 

ssS  r\s^shaft 


Practically,  the  values  of  are  approximated  as  zero  in  this 
calculation.  The  error  covariance  matrix  (P)  for  the  next  cycle 
is  then  updated  with  Equation  26: 

Pn..,=A-  ZK^HJP  (26) 

ssS  Ds^shaft 


It  should  be  noted  that,  in  the  context-sensitive  pose 
computation,  dynamic  adjustment  of  the  G-matrix  affects 
matrices  Q  (Equation  18)  and  R  (Equation  23). 


3.  Particle  Filter 

The  third  type  of  the  fusing  methods  implements  the 
Monte  Carlo  localization  (MCL)  method,  another  probabilistic 
approach;  we  call  this  fuser  the  Particle  Filter.  Recently,  the 
MCL  has  been  used  extensively  in  the  robotics  community  to 


The  first  step  in  the  prediction  phase  is  to  project  the  pose 
of  each  particle  based  on  the  process  model  (Equation  28): 

O,  =  fpp(o,_i,u,,w,A  (28) 


Similar  to/pE  in  Equation  12, /pp  is  the  process  model  for  the 
Particle  Filter,  u  is  the  control  input  (approximated  with 
Arshaft),  and  w  is  the  estimated  process  noise,  /pp  can  be 
described  by  the  following  equation  (Equation  29): 


A/'cos(6»„,_i  -t^)cos(^„,.i  -t^) 

,  Ad, 

Al  cos(6>„,_i 

A/9 

A/'sin(6'„,^i  +-A) 

A(t>, 

A9, 

XWt 


(29) 


where  A(^,  AO,  and  A  ((rare  <p,  0,  and  ((z  components  of  Ar shaft, 
respectively;  A/'  is  the  Euclidean  norm  of  Avshaft  with  the 
process  noise  being  taken  account.  More  specifically,  A/'  can 
be  obtained  from  this  equation  (Equation  30): 

Al'  =  GaussSample(A/,  ) )  (30) 

where  GaussSample  is  a  function  that  draws  a  sample  from  the 
normal  density  having  the  mean  and  standard  deviation 
specified  in  its  first  and  second  input  parameters,  respectively; 
Al  is  the  Euclidean  norm  of  Ar  shaft  (Equation  14);  Mean  is  a 
function  that  returns  an  average  value  of  its  input  vector;  and 
Xshaft  is  the  variances  stored  in  the  second  column  of  the 
discounted  G-matrix  for  the  shaft-encoder.  Notice  that  /pe  in 
the  EKF  (Equation  13)  and/pp  in  the  Particle  Filter  (Equation 
29)  are  similar.  However,  the  difference  is  that,  while  the 
process  noise  (w)  is  linearly  added  to  /pe,  the  process  noise  in 
/pp  is  already  taken  into  account  when  computing  the  Euclidean 
norm  of  Arshaft  (i-c.,  AP). 

In  the  update  phase  of  the  Particle  Filter,  the  pose  of  each 
particle  is  refined  based  on  the  sensor  measurements.  It  is  done 
by  first  calculating  weight  (cOg)  for  each  particle  using 
Equation  3 1 : 
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n  (31) 

ssS  n  s^shaft  \i=l  ) 

where  5  is  a  set  of  all  n  sensors  that  have  updated  their 
measurements  since  the  previous  cycle;  d  is  the  dimension  of  o 
(which  is  six);  and/L  is  a  function  that  returns  the  likelihood  of 
a  sample  (specified  in  the  first  input  parameter)  given  a 
measurement  (specified  in  the  second  input  parameter);  our/L 
assumes  the  normal  density,  and  hence  its  standard  deviation  is 
specified  by  the  third  input  parameter.  Once  the  weights  are 
computed  for  all  N  particles,  they  are  normalized  (&Jo)  and 
combined  as  a  vector  (fi)  as  shown  in  Equation  32: 

^  =  Wol  •••  (32) 

The  next  step  is  re-sampling;  a  new  particle  (o„en,)  is  drawn 
from  set  O  using  Equation  33: 

=  0{-.,  IRandom(fl))  (33) 


where  IRandom  is  a  function  that  returns  the  index  of  an 
element  that  was  picked  by  weighted  random  sampling  from 
the  input  vector  where  the  values  of  the  input  vector  are 
sampling  weights.  Equation  33  is  repeated  N  times  to  form  a 
new  set  of  N  particles  (G„e„). 

Einally,  each  component  of  the  final  pose  (X^)  for  the 
Particle  Eilter  is  computed  by  taking  the  average  of  the 
appropriate  element  in  all  new  particles  (Equation  34): 


Mean(G_(l,:)) 

Mean(0_(2,:)) 


(35) 


where  d  is  the  dimension  of  (which  is  six). 

It  should  be  noted  that,  in  the  context-sensitive  pose 
computation,  dynamic  adjustment  of  the  G-matrix  affects  the 
value  of  A/'  (Equation  30)  and  ai  (Equation  31). 


III.  Implementation 

The  context-sensitive  pose  computation  described  above, 
including  the  three  fusing  methods,  was  implemented  within 
HServer  (Eigure  1),  one  of  the  components  of  MissionLab  [20, 
21].  HServer  is  a  UNIX  process  that  communicates  with 
attached  hardware  devices  via  serial  ports  or  TCP/IP  socket 
connections.  Eor  example,  HServer  can  control  a  physical 
robot  by  executing  commands  that  are  issued  from  Robot 
Executable,  another  MissionLab  process  where  behaviors  are 
computed. 

Another  functionality  of  HServer  is  to  marshal  sensory 
information  from  attached  sensors  and  report  it  to  Robot 
Executable.  Such  sensory  information  includes  the  pose  of  the 
robot.  More  specifically,  pose  computation  is  done  in  a 
module  called  PoseCalculator  in  HServer.  PoseCalculator 
gathers  the  latest  readings  from  the  GPS,  compass,  IMU  and 
shaft-encoder  (if  they  are  enabled),  and  attempts  to  compute 
the  best  estimate  of  the  current  pose  based  on  those.  Indeed, 
PoseCalculator  is  where  the  proposed  context-sensitive  pose 


computation  scheme  was  integrated.  Eigure  2  depicts  the 
process  of  the  context-sensitive  pose  computation  in 
PoseCalculator.  As  shown  in  the  figure,  a  module  called  Pose 
Manager  implements  Equations  2  and  6  in  order  to  compute 
the  converted  pose  for  the  sensor  (Z^)  and  its  (initial)  grade 
(Gj),  respectively.  Discounting  of  the  grade  (Gj)  is  done  in 
Sensory  Situational  Context  (Equation  7).  All  asynchronously 
computed  Zj  and  G  ^  from  different  sensors  arrive  at  Sensory 
Data  Bus,  from  which  Sensor  Fuser  grabs  the  latest  values 
every  computational  cycle.  Sensor  Fuser  (Equation  9)  is  where 
the  fused  pose  (Z^)  is  computed  by  employing  one  of  the  three 
fuser  methods  (i.e..  Maximum  Confidence,  EKE,  and  Particle 
Eilter). 


HServer 


Figure  1 :  HSer\>er 


Equations  2  &  6  Equation  7 


last  pose 


Figure  2:  PoseCalculator 


IV.  Evaluation 

A.  Experimental  Hypotheses 

An  outdoor  experiment  was  conducted  in  order  to 
determine  whether  incorporation  of  contextual  information  and 
domain  knowledge  helps  the  accuracy  of  localization.  More 
specifically,  the  experiment  was  designed  to  assess  the 
following  hypotheses: 

Hypothesis  1: 

If  adequate  real-time  contextual  information  and  domain 
knowledge  is  incorporated,  the  robot’s  pose  computed  by  a 
simple  greedy  fusing  method  can  be  as  accurate  as  the  one 
computed  by  the  conventional  probabilistic  localization 
methods. 


Hypothesis  2: 

The  conventional  probabilistic  localization  methods  can 
improve  their  accuracy  of  the  robot’s  pose  if  real-time 
contextual  information  and  domain  knowledge  are 
incorporated. 

B.  Experimental  Area 

The  outdoor  experiment  was  conducted  at  the  top  level  of 
a  parking  deck  in  the  Georgia  Tech  campus  to  test  the  above 
hypotheses.  As  shown  in  Figure  3,  the  area  was  about  60 
meters  wide  and  about  90  meters  long.  Six  waypoints  were 
selected  in  the  area:  Start  Point  <53.1,  37.4>,  A  <53.1,  63.3  >, 
B  <53.1,  97.7  >,  C  <23.3,  97.7>,  D  <23.3,  63.3>,  and  E  <23.3, 
37.4>  (Note:  the  coordinates  <x:,  y>  are  in  meters). 


Known  areas  of 
distorted  magnetic 
fields  (due  to  large 
steel  girders) 


Figure  3:  Experimental  Area 


C.  Contextual  Information  and  Domain  Knowledge  Used 
Tables  1  and  2  show  the  values/rules  of  contextual 
information  (Q  and  domain  knowledge  (function  /d)  being 
used  during  the  outdoor  experiment  when  they  were 
dynamically  adjusted.  The  values  of  the  grade  (G)  when  the  G- 
matrix  is  statically  defined  are  shown  in  Table  3.  Table  4 
shows  the  values  of  discount  factors  (Equation  8).  All  these 
values  used  here  are  determined  based  on  hardware 
specifications  and  through  trial-and-error  during  the  testing. 
However,  it  should  be  noted  that  calibration  of  such 
parameters  is  a  delicate  process,  and  we  do  not  guarantee  that 
they  are  perfectly  optimized.  Nevertheless,  they  are,  to  the  best 
of  our  knowledge,  adequately  tuned. 


Table  1 :  Real-Time  Contextual  Information  (C) 


GPS 

Shaft-Encoder 

Cl  =  RT-20  value 

Cl  =  translational  speed  of  robot 

C3  =  angular  speed  of  robot 

Cl  =  translational  speed  of  robot 

Cl  =  angular  speed  of  robot 

Compass 

IMU 

Cl  =  angular  speed  of  robot 

C  =  0  (empty  set) 
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Table  2:  Dynamical  Adjustment  of  Grades  (G)  via  Domain  Knowledge  (/b) 


GPS 

Compass 

if  Cl  =  0  then 

^[x,y,z}  —  ~ 

if  Cl  >=  0  and  C3  =  0 

=  E  1 

else 

=  0,  c/ {p,e,y/}  =  129600 

end 

t^[x,y,z]  ~  O'  (^{ti>,d,yA  ~  1 
=  1000000 
if  Cl  =  0  then 
{ipAy/}  -  360 

else 

d‘[i,,e.w)  =  129600 

End 

else  if  Cl  =  1  then 

^{x,y,z)  ~  1-0,  {x,y,z)  —  0.3 

if  C2  >=  0  and  C3  =  0 
(^[^.,0,1!/)  —  E  {^,9,y/}  —  4 

else 

g({(1>Aw]  =  0,  {^Aw)  ~  129600 

end 
end 

Shaft-Encoder 

t^[x,y,z]  ~  0,  C({ti)A,yA  ~  1 
if  Cl  =  0  and  C2  =  0  then 

0^{wi  =  0.0001,  =  0.001 

else 

^ {x,y,z)  —  0.001,  {^A,yA  ~  1 

End 

Note:  GPS  readings  are  ignored 
when  RT-20  is  2  or  greater  (same  for 
the  static  case). 

IMU 

t^[x,y,z]  ~  0,  CC{il)^Q^y/]  =  1 

c^(w)  =  1000000,  =  1 

Table  3:  Static  Grades  (G) 


GPS 

Shaft-Encoder 

t^{x,y,z}  ~  E  —  0 

[x.y.z]  =  0.25,  (f  =  3600 

t^{x,y,z]  ~  E  0C{y)^9,\ff]  =  \ 

<f  ix,y,z]  =  0.01,  =  1 

Compass 

IMU 

t^{x,y,z]  ~  0,  o:[^A<>/i]  ~  0 

=  1000000,  =  8100 

t^[x,y,z]  ~  0,  CC{(lf^Q^y/]  =  1 

[x^yyZ]  =  1000000,  {pAM  ~  1 

Table  4:  Discount  Factors  (y)  in  the  Weighting  Matrix  (T) 


GPS 

Compass 

IMU 

Shaft-Encoder 

Tops  =  0.9 

Ycompass  —  0.8 

?}«(/=  0.75 

Yshaft  —  0.5 

D.  Hardware 

Both  HServer  and  Robot  Executable  ran  on  the  onboard 
dual  processors  (Pentium  III,  1  GHz)  of  an  ATRV-Jr  (iRobot 
Corporation)  during  execution.  The  ATRV-Jr  was  equipped 
with  a  differential  GPS  (ProPak  by  NovAtel,  Inc.),  a  compass 
(3DM-G  by  MicroStrain,  Inc.),  an  IMU  (IMU400CC-200  by 
Crossbow  Technology,  Inc.),  and  internal  shaft-encoders.  In 
addition,  two  sets  of  onboard  laser  scanners  (LMS  200-30106 
by  SICK,  Inc.)  were  used  to  measure  the  ground  truth  of  the 
current  pose  (explained  below).  The  base  station  for  the 
differential  GPS  was  placed  8  meters  south  of  Start  Point. 

E.  Methods 

In  order  to  test  the  above  experimental  hypotheses,  an 
autonomous  waypoint-following  mission  was  created  and 
executed  by  MissionLab.  In  this  mission,  the  robot  followed 
the  six  points  by  the  order  of  Start  Point,  A,  B,  C,  D,  E,  D,  C, 
B,  A,  and  then  back  to  Start  Point.  Here,  the  segment  from 
Start  Point  to  Point  B  is  called  Leg  1 ;  the  segments  B  ^  C,  C 
— >  E,  E  — >  C,  C  — ^  B,  and  B  ^  Start  Point  are  called  Legs  2, 
3,  4,  5,  and  6,  respectively.  During  the  mission,  the  robot  was 
always  commanded  to  run  with  its  full-speed  (approximately  2 
m/s)  unless  making  a  point-turn  at  a  waypoint. 

To  ensure  GPS  disruption,  the  differential  signals  from  the 
base  station  to  the  robot  were  physically  cut  off  when  the  robot 
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was  at  Leg  2  (from  Point  B  to  Point  C).  This  allowed  the  RT- 
20  value  [7]  of  the  differential  GPS  to  degrade  gradually  from 
0  to  8,  simulating  realistic  deterioration  of  the  GPS  accuracy. 
During  the  return  trip,  the  transmission  of  differential  signals 
was  resumed  at  Leg  5  (from  Point  C  to  Point  B).  Furthermore, 
during  Legs  1,  3,  4,  and  6,  the  robot  had  to  go  through  the 
areas  where  the  magnetic  fields  were  distorted  hy  steel  girders 
laying  underneath  the  floor,  affecting  the  performance  of  the 
compass  in  a  nonlinear  manner. 

In  order  to  determine  the  accuracy  of  the  pose  computed 
by  the  system  with  respect  to  the  ground  truth,  the  computed 
pose  and  readings  from  laser  scanners  were  recorded  for  every 
second  during  Legs  1,  4,  and  6.  The  set  of  the  two  lasers  can 
acquire  722  readings  (covering  360°)  with  an  update  rate  of 
four  times  per  second.  As  shown  in  Figure  3,  during  Legs  1,  4, 
and  6,  the  robot  moved  along  with  the  flat  walls  laying  in  the 
North-South  direction,  namely.  Wall  R  and  Wall  L.  Since  the 
coordinates  of  those  walls  were  known,  one  can  calculate  the 
expected  distance  from  the  pose  to  the  wall,  and  compare  it 
with  the  actual  distance  measured  by  the  laser  scanners.  The 
difference  between  the  expected  distance  and  the  actual 
distance  is  defined  here  as  a  distance  error.  Moreover,  since 
the  angle  of  the  direction  of  which  the  laser  scanners  found  the 
closest  distance  to  the  wall  was  known,  one  can  also  calculate 
the  actual  heading  of  the  robot  with  respect  to  the  wall  (i.e., 
with  respect  to  the  ground  truth).  The  difference  between  the 
actual  heading  and  the  expected  heading  computed  by  the 
system  is  defined  here  as  a  heading  error.  In  others  words,  in 
this  experiment,  the  accuracy  of  the  pose  computed  by  the 
system  was  determined  by  the  distance  and  heading  errors. 

Two  conditions  were  tested  for  each  of  the  three  fusing 
methods  (the  Maximum  confidence,  EKF,  and  Particle  Filter); 
the  first  condition  is  context-free,  that  is  when  the  G-matrix 
(Equation  5)  is  fixed;  and  the  second  condition  is  context- 
sensitive,  that  is  when  the  G-matrix  is  dynamically  adjusted  by 
Equation  6  (explained  in  Section  IV. C).  In  order  to  be 
statistically  significant,  20  runs  of  the  waypoint-following 
mission  were  recorded  for  every  condition  (i.e.,  the  total  of 
120  runs  were  recorded  for  the  six  conditions).  As  a  standard 
practice,  we  consider  a  difference  of  two  means  to  be 
significant  if  the  p-value  of  the  associated  ANOVA  test  is  less 
than  0.05  (5%). 

F.  Results 

The  average  heading  and  distance  errors  for  all  conditions 
are  plotted  against  the  leg  number  in  Eigures  4  and  5, 
respectively.  The  error  bars  in  the  figures  denote  95  percent 
confidence  intervals. 

Regarding  Hypothesis  1,  the  greedy  fusing  method 
(Maximum  Confidence)  with  the  context-sensitive  condition 
(dynamic  G-matrix)  was  compared  against  the  context-free 
(fixed  G-matrix)  conventional  probabilistic  localization 
methods:  namely,  the  EKF  and  Particle  Filter.  At  Leg  1,  the 
one-way  ANOVA  test  showed  that  the  context-sensitive 
Maximum  Confidence  had  significantly  less  heading  error  than 
the  context-free  EKF  (F  =  4.167,  p  <  0.048;  the  error  bars 


slightly  overlapped).  However,  there  was  no  significant 
difference  if  compared  to  the  heading  error  produced  by  the 
context-free  Particle  Filter.  At  Leg  4  (the  GPS  shadow),  the 
average  heading  error  of  the  context-sensitive  Maximum 
Confidence  was  not  significantly  different  from  the  context- 
free  EKF  and  Particle  Filter.  At  Leg  6  (the  final  leg),  the 
context-sensitive  Maximum  Confidence  produced  significantly 
less  heading  error  compared  to  the  context-free  EKF  (F  = 
9.845,  p  <  0.003)  and  the  context-free  Particle  Filter  (F  = 
6.961,  p  <  0.012;  error  bars  slightly  overlapped).  However,  the 
context-sensitive  Maximum  Confidence  had  no  significant 
distance  errors  over  the  context-free  EKF  and  Particle  Filter  at 
all  legs.  Overall,  these  results  support  the  first  hypothesis  and 
indicate  that  with  the  addition  of  the  proper  contextual 
information  and  domain  knowledge,  a  simple  greedy  fusing 
method  can  achieve  accuracies  meeting  and  in  some  cases 
exceeding  that  of  the  two  conventional  probabilistic  filters 
used  in  these  experiments. 

On  the  other  hand,  the  second  hypothesis  was  not 
supported  by  the  current  data.  In  other  words,  in  both 
probabilistic  filters,  the  heading  and  distance  errors  when 
using  contextual  information  in  the  form  of  dynamic  variances 
(context-sensitive)  did  not  exhibit  significant  differences  if 
compared  with  the  context-free  ones. 


Average  Heading  Error 


Average  Distance  Error 


V.  Conclusions  and  Future  Work 

This  work  details  context-sensitive  pose  computation  and 
empirically  evaluates  it  within  the  framework  of  a  localization 
task  in  an  urban  environment.  In  this  task,  the  robot  must 
provide  accurate  localization  information  even  in  the  event  of 
sensor  drop-out  and  in  the  presence  of  non-linear  error  sources 
over  the  span  of  numerous  waypoint-following  missions.  The 
utility  of  the  computational  scheme  is  illustrated  by  the 
performance  of  the  Maximum  Confidence,  based  purely  on 
this  context-sensitive  information,  matches  or  even  exceeds  the 
performance  of  the  conventional  probabilistic  localization 
methods  (i.e.,  the  EKF  and  Particle  Filter).  Further,  it  has  been 
shown  to  be  robust  under  a  wide  variety  of  sensor  noise  such 
as  that  produced  by  GPS  dropout  and  the  non-linear  sensor 
noise  produced  by  the  large  steel  girders  present  in  the 
experimental  arena. 

On  the  other  hand,  our  evaluation  determined  that  the 
performances  of  the  probabilistic  filters  are  not  affected 
significantly  by  the  utilization  of  the  contextual  information  in 
the  form  of  dynamic  variances.  A  few  causes  are  speculated: 
(1)  The  domain  knowledge  (i.e.,  adjustment  of  variances)  was 
not  adequate;  (2)  the  probabilistic  filters  were  so  efficiently 
formulated  that  the  extra  information  did  not  add  any  value;  or 
(3)  the  navigational  task  and/or  environment  was  too  simple. 
An  additional  set  of  experiments  should  be  conducted  in  order 
to  solve  this  predicament. 

Furthermore,  in  this  study,  the  performance  of  our 
computational  scheme  was  measured  by  the  accuracy  of  the 
output  pose  with  respect  to  the  ground  truth.  In  a  real  urban 
outdoor  navigational  task,  however,  how  effectively  the  robot 
can  accomplish  the  assigned  task  is  also  important;  such 
effectiveness  includes  its  ability  to  arrive  to  a  waypoint  quickly 
or  ability  to  overcome  presence  of  static  and/or  dynamic 
obstacles  without  being  disoriented.  In  other  words,  the 
computational  scheme  should  be  also  evaluated  in  terms  of 
behavioral  accuracies. 

A  possible  extension  of  this  work  relates  to  Murphy’s 
action-oriented  perceptual  architecture  [8]  described  in  Section 
I.  By  adding  some  high-level  planning  mechanism,  dynamical 
switching  or  even  blending  of  the  fusing  methods  themselves  is 
also  possible,  and  such  an  extension  may  be  advantageous  in 
more  complex  and/or  dynamic  environments. 
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